////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "Node.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace ai {
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
Node::Node( int _iX, int _iY ) :
    X( _iX ),
    Y( _iY ),
    F( 0.0f ),
    G( 0.0f ),
    parent( NULL )
{
}
////////////////////////////////////////////////////////////
/// Copy Constructor
////////////////////////////////////////////////////////////
Node::Node( const Node& _Copy ) :
    X( _Copy.X ),
    Y( _Copy.Y ),
    F( _Copy.F ),
    G( _Copy.G ),
    parent( _Copy.parent )
{
}

////////////////////////////////////////////////////////////
/// Setup a Node
////////////////////////////////////////////////////////////
void Node::Setup( int _iX, int _iY ) {
    X       = _iX;
    Y       = _iY;
    F       = 0.0f;
    G       = 0.0f;
    parent  = NULL;
}
    
////////////////////////////////////////////////////////////
/// Compute the distance to another node
////////////////////////////////////////////////////////////
float Node::DistanceTo( const Node& _Other ) const {
    int iDistX = _Other.X - X,
        iDistY = _Other.Y - Y;
    return sqrt( (float)(iDistX * iDistX + iDistY * iDistY) );
}

////////////////////////////////////////////////////////////
/// Compute the squared distance to another node
////////////////////////////////////////////////////////////
float Node::SquaredDistanceTo( const Node& _Other ) const {
    int iDistX = _Other.X - X,
        iDistY = _Other.Y - Y;
    return (float)(iDistX * iDistX + iDistY * iDistY);
}

/*********************** Operators ************************/
bool Node::operator==( const Node& _Other ) const {
    return _Other.X == X && _Other.Y == Y;
}

bool Node::operator!=( const Node& _Other ) const {
    return _Other.X != X || _Other.Y != Y;
}

bool Node::operator>( const Node& _Other ) const {
    if ( F > _Other.F ) {
        return true;
    } else {
        return ( F == _Other.F && G < _Other.G );
    }
}

bool Node::operator>=( const Node& _Other ) const {
    if ( F > _Other.F ) {
        return true;
    } else {
        return ( F == _Other.F && G <= _Other.G );
    }
}

bool Node::operator<( const Node& _Other ) const {
    if ( F < _Other.F ) {
        return true;
    } else {
        return ( F == _Other.F && G > _Other.G );
    }
}

bool Node::operator<=( const Node& _Other ) const {
    if ( F < _Other.F ) {
        return true;
    } else {
        return ( F == _Other.F && G >= _Other.G );
    }
}

/********************* Static members *********************/
int Node::UniqueKey( Node* _pNode, int _iOffset ) {
    // TODO See if we have better than this...
    return _pNode->X + _pNode->Y * _iOffset;
}

    } // namespace ai
} // namespace smeg
